#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(Sensor, in2,    gyro,           sensorGyro)
#pragma config(Sensor, in3,    armMeasure,     sensorPotentiometer)
#pragma config(Sensor, in4,    armSetPot,      sensorPotentiometer)
#pragma config(Sensor, dgtl2,  ,               sensorDigitalOut)
#pragma config(Sensor, dgtl7,  brake,          sensorDigitalOut)
#pragma config(Sensor, dgtl8,  trayTilt,       sensorDigitalOut)
#pragma config(Sensor, dgtl9,  limitSwitch,    sensorTouch)
#pragma config(Motor,  port1,           backleft,      tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port2,           frontleft,     tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port3,           armbottomleft, tmotorVex393, openLoop)
#pragma config(Motor,  port4,           armtopleft,    tmotorVex393, openLoop)
#pragma config(Motor,  port5,           intakeleft,    tmotorVex393, openLoop)
#pragma config(Motor,  port6,           intakeright,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port7,           armtopright,   tmotorVex393, openLoop)
#pragma config(Motor,  port8,           armbottomright, tmotorVex393, openLoop)
#pragma config(Motor,  port9,           frontright,    tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor,  port10,          backright,     tmotorVex393HighSpeed, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "Driver.c"

//****************************Declarations because C is dum***************************************//
void flipDownAuton();
void leftAuton();
void rightAuton();
void centerStack();
void rightAutonSimple();
void leftAutonSimple();
void underGoalPickup();
void leftAutonCombined();

/*****************************************************************************************Competition Code*****************************************************************************************/
/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                          Pre-Autonomous Functions
//
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton(){
	//StartTask(startLCD,6); LCD removed
	displayOverride=true;
	setOverrideStrings(sInit,sNoMove);

	//  initEncoders(); LDR Assume not needed Jan 4, 2012 since encoders were removed
	initGyro();
	initPIDs();
	StartTask(findPIDs);
	//StartTask(armLock);

	displayOverride=false;
}//end pre-auton

/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                                 Autonomous Task
//
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous(){


	walk=strafe=turn=0;
	armMove(0);
	autonOverride=true;
	//StartTask(dialAnArm);
	StartTask(gyroControl);
	StartTask(armControl);
	if(SensorValue[limitSwitch]==1){ //left Auton: Switch in
		flipDownAuton();
		centerStack();
		//leftAutonSimple();
		//underGoalPickup();
		leftAutonCombined();
	}
	else{//right auton: Switch out
		flipDownAuton();
		centerStack();
		rightAutonSimple();
		underGoalPickup();
	}

	//StopTask(dialAnArm);
	StopTask(gyroControl);
	StopTask(armControl);
	autonOverride=false;
	armSetPointSet=false;
	armMove(0);
}//end autonomous

/////////////////////////////////////////////////////////////////////////////////////////
//
//
//                                 User Control Task
//
//
/////////////////////////////////////////////////////////////////////////////////////////
task usercontrol(){

	StopTask(gyroControl);
	StopTask(armControl);
	autonOverride=false;
	armSetPointSet=false;
	armMove(0);

	bool tiltPrev=false;
	bool tilt=false;

	while (true)
	{
		//drive inputs
		walk=vexRT[Ch3];
		strafe=vexRT[Ch4];
		turn=vexRT[Ch1];

		//Arm inputs
		armPower=-vexRT[Ch2];

		//intake inputs
		if(vexRT[Btn6U]!=0) inPower=127;
		else if(vexRT[Btn6D]!=0) inPower=-127;
		else inPower=0;

		//Pivot input
		if(vexRT[Btn8L]){
			tilt=true;
		}
		else{
			tilt=false;
		}
		if(tiltPrev&&!tilt){
			tiltPrev=false;
		}
		else if(!tiltPrev&&tilt){
			toggleTrayTilt();
			tiltPrev=true;
		}

		if(vexRT[Btn8D]==1){
			SensorValue[brake]=1;
		}
		else{
			SensorValue[brake]=0;
		}


		//scale
		scaleInputs();


		//Drive
		driveCheesy(walk,strafe,turn);

		//Arm
		armMove(armPower);

		//Bucket
		IntakeMove(inPower);

		while(vexRT[Btn7U]!=0){
			stopAllMotors();
		}

		wait1Msec(15);
	}
}//end usercontrol

void flipDownAuton(){
	armSetPoint=armMin-200;
	armSetPointSet=true;
	IntakeMove(128);
	wait1Msec(200);
	driveMecanum(128,0,0);
	//walk=128;
	wait1Msec(200);
	driveMecanum(-10,0,0);
	//walk=-10;
	wait1Msec(100);
	IntakeMove(0);
	driveMecanum(0,0,0);
	//walk=0;
	wait1Msec(100);
}

void centerStack(){
	//move to center stack
	driveMecanum(60,0,0);
	//walk=60;
	IntakeMove(127);
	armSetPoint=armMin-200;
	wait1Msec(2000);
	//drive back a bit
	driveMecanum(-30,0,0);
	//walk=-30;
	wait1Msec(500);
	//intake for a while
	driveMecanum(0,0,0);
	//walk=0;
	wait1Msec(1000);
	IntakeMove(0);
}

void leftAuton(){
}

void rightAuton(){
	//	int gyroPos=getGyro();
	//raise arm and turn left to position
	armSetPoint=armScore;
	//gyroSetPoint=gyroPos+200;
	wait1Msec(800);
	//drive forward a bit
	driveMecanum(40,0,0);
	wait1Msec(700);
	//turn to goal
	driveMecanum(0,0,0);
	gyroSetPoint=getGyro()-220;
	wait1Msec(700);
	//drive to goal
	driveMecanum(60,0,0);
	wait1Msec(800);
	driveMecanum(0,0,0);
	//dump
	IntakeMove(-128);
}

void rightAutonSimple(){
	//raise arm
	armSetPoint=armScore;
	wait1Msec(800);
	//drive to goal
	driveMecanum(60,0,0);
	wait1Msec(800);
	driveMecanum(0,0,0);
	//dump
	IntakeMove(-128);
	wait1Msec(5000);
	IntakeMove(0);
}

void leftAutonSimple(){
	//return home
	IntakeMove(50);
	driveMecanum(-128,0,0);
	wait1Msec(600);
	//reposition
	driveMecanum(0,0,0);
	wait1Msec(1500);
	armSetPoint=armScore;
	wait1Msec(200);
	//drive to goal
	driveMecanum(60,0,0);
	wait1Msec(2000);
	driveMecanum(0,0,0);
	//dump
	IntakeMove(-128);
	wait1Msec(2000);
	IntakeMove(0);
}

void underGoalPickup(){
	//back up a bit and lower arm
	driveMecanum(-60,0,0);
	wait1Msec(500);
	armSetPoint=armMin-200;
	wait1Msec(200);
	driveMecanum(0,0,0);
	wait1Msec(300);
	//Intake and drive forward
	IntakeMove(128);
	driveMecanum(50,0,0);
	wait1Msec(1500);
	//rake
	driveMecanum(-20,0,0);
	wait1Msec(700);
	driveMecanum(0,0,0);
	IntakeMove(0);
}

void leftAutonCombined(){
	//return home
	IntakeMove(50);
	driveMecanum(-128,0,0);
	wait1Msec(600);
	//reposition
	driveMecanum(0,0,0);
	wait1Msec(1500);
	//drive to goal and intake
	driveMecanum(60,0,0);
	IntakeMove(128);
	wait1Msec(2400);
	driveMecanum(0,0,0);
	//back up a bit and raise arm (in several steps)
	driveMecanum(-60,0,0);
	armSetPoint=armMin+200;
	wait1Msec(650);
	armSetPoint=armScore;
	wait1Msec(600);
	driveMecanum(0,0,0);
	wait1Msec(100);
	//drive forward
	driveMecanum(60,0,0);
	wait1Msec(850);
	driveMecanum(0,0,0);
	//dump
	IntakeMove(-128);
	wait1Msec(2000);
	IntakeMove(0);
}
